/**
  ****************************(C) COPYRIGHT 2025 CZJ****************************
  * @file       chassis_omni.c/h
  * @brief      全向轮底盘控制器
	* @note       包括初始化，目标量更新、状态量更新、控制量计算与直接控制量的发送
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2025/09/03      CZJ		          1. 完成。
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 CZJ****************************
**/

#include "robot_param.h"
#if (CHASSIS_TYPE == CHASSIS_OMNI_WHEEL)
#ifndef CHASSIS_OMNI_H
#define CHASSIS_OMNI_H
#include "bsp_can.h"
#include "bsp_rc.h"
#include "pid.h"
#include "INS_task.h"
#include "motor.h"
#include "RM_Ctrl.h"
#include "can_receive.h"
#include "math.h"

/*-------------------- Structural definition --------------------*/
typedef enum {
    CHASSIS_LOCK,      //底盘锁定，所有轮子速度设定为0
    CHASSIS_SINGLE,    //只有底盘的模式
    CHASSIS_FOLLOW,    //云台跟随模式
} ChassisMode_e;

/**
 * @brief  底盘轮子PID
 */
 typedef struct
{
    pid_type_def wheel_velocity[4];//麦轮速度解算PID

    pid_type_def follow; //云台跟随PID
} PID_t;   

/**
 * @brief  底盘期望
 */
typedef struct
{
    float vx;
    float vy;
    float vw;
} Reference_t;

/**
 * @brief  底盘数据结构体
 * @note   底盘坐标使用右手系，前进方向为x轴，左方向为y轴，上方向为z轴
 */
typedef struct
{
    const RC_ctrl_t *rc;  						// DBUS遥控器指针
		const SBUS_RC_Ctrl_t *sbus_rc; ;  // SBUS遥控器指针
		const COMM_RC_Ctrl_t *comm_rc;  	// 上板指令数据指针
    const INS_t *IMU;     // imu数据
    ChassisMode_e mode;   // 底盘模式

    /*-------------------- Motors --------------------*/
    Motor_s wheel[4];   //底盘电机

    /*-------------------- Values --------------------*/
    Reference_t reference; 		//最终目标量
    Reference_t reference_rc;	//获取遥控器目标量

    float feedback[4];		//当前值
    float set[4];					//目标值

    float yaw_delta;
} Chassis_s;


extern void ChassisInit(void);

extern void ChassisSetMode(void);

extern void ChassisObserver(void);

extern void ChassisReference(void);

extern void ChassisConsole(void);

extern void ChassisSendCmd(void);

#endif 
#endif 
